# improved_preprocessing.py
import numpy as np


def extract_detailed_features(sequence):
    """提取更详细的特征，特别关注手指形状"""
    if len(sequence) < 2:
        return np.zeros(80)

    start_frame = sequence[0]
    end_frame = sequence[-1]

    # 1. 基础位置变化特征
    position_movement = end_frame - start_frame

    # 2. 手指弯曲特征 - 专门针对抓取动作
    finger_bending_features = extract_finger_bending_features(start_frame, end_frame)

    # 3. 手掌形状变化特征
    palm_shape_features = extract_palm_shape_features(start_frame, end_frame)

    # 4. 运动轨迹特征
    movement_features = extract_movement_features(sequence)

    # 组合所有特征
    detailed_features = np.concatenate([
        position_movement,  # 63维：位置变化
        finger_bending_features,  # 5维：手指弯曲
        palm_shape_features,  # 5维：手掌形状
        movement_features  # 7维：运动轨迹
    ])

    return detailed_features


def extract_finger_bending_features(start_frame, end_frame):
    """提取手指弯曲特征"""
    features = []

    # 每个手指的指尖到手掌根部的距离变化
    finger_indices = [
        [0, 8],  # 拇指：手腕到拇指尖
        [0, 12],  # 食指：手腕到食指尖
        [0, 16],  # 中指：手腕到中指尖
        [0, 20],  # 无名指：手腕到无名指尖
        [0, 24]  # 小指：手腕到小指尖
    ]

    # 验证帧数据长度
    expected_length = 25 * 3  # 25个关键点，每个3个坐标
    if len(start_frame) < expected_length or len(end_frame) < expected_length:
        return np.zeros(5)  # 返回默认特征

    for start_idx, end_idx in finger_indices:
        # 计算起始距离
        start_dist = calculate_distance(start_frame, start_idx, end_idx)
        # 计算结束距离
        end_dist = calculate_distance(end_frame, start_idx, end_idx)
        # 距离变化（抓取动作应该减小）
        features.append(start_dist - end_dist)

    return np.array(features)


def extract_palm_shape_features(start_frame, end_frame):
    """提取手掌形状特征"""
    features = []

    # 手掌区域的关键点
    palm_points = [0, 1, 5, 9, 13, 17]  # 手腕和各手指根部

    # 计算手掌区域的"紧凑度"变化
    start_compactness = calculate_compactness(start_frame, palm_points)
    end_compactness = calculate_compactness(end_frame, palm_points)
    features.append(end_compactness - start_compactness)  # 抓取时紧凑度增加

    # 各手指根部到手腕的距离变化
    for point_idx in [5, 9, 13, 17]:
        start_dist = calculate_distance(start_frame, 0, point_idx)
        end_dist = calculate_distance(end_frame, 0, point_idx)
        features.append(end_dist - start_dist)

    return np.array(features)


def extract_movement_features(sequence):
    """提取运动轨迹特征"""
    if len(sequence) < 3:
        return np.zeros(7)

    # 手掌中心点轨迹（使用手腕点）
    wrist_trajectory = [frame[0:3] for frame in sequence]  # 手腕的x,y,z

    # 计算运动统计特征
    movements = []
    for i in range(1, len(wrist_trajectory)):
        movement = np.linalg.norm(wrist_trajectory[i] - wrist_trajectory[i - 1])
        movements.append(movement)

    features = [
        np.mean(movements),  # 平均运动速度
        np.std(movements),  # 运动稳定性
        np.max(movements),  # 最大运动速度
        np.sum(movements),  # 总运动距离
        movements[-1] if movements else 0,  # 最终运动速度
        len(sequence),  # 序列长度
        calculate_trajectory_curvature(wrist_trajectory)  # 轨迹曲率
    ]

    return np.array(features)


def calculate_distance(frame, idx1, idx2):
    """计算两个关键点之间的欧氏距离"""
    # 检查帧数据长度是否足够
    required_length = max(idx1, idx2) * 3 + 3
    if len(frame) < required_length:
        # 如果帧数据不够长，返回默认距离0
        return 0.0

    # 安全地提取关键点坐标
    point1 = frame[idx1 * 3:idx1 * 3 + 3]
    point2 = frame[idx2 * 3:idx2 * 3 + 3]

    # 验证提取的点是否有效
    if len(point1) != 3 or len(point2) != 3:
        return 0.0

    # 计算欧氏距离
    return np.linalg.norm(point1 - point2)


def calculate_compactness(frame, point_indices):
    """计算一组点的紧凑度（平均到中心点的距离）"""
    points = []
    for idx in point_indices:
        points.append(frame[idx * 3:idx * 3 + 3])

    points = np.array(points)
    center = np.mean(points, axis=0)
    distances = [np.linalg.norm(p - center) for p in points]
    return np.mean(distances)


def calculate_trajectory_curvature(trajectory):
    """计算轨迹曲率（简单版本）"""
    if len(trajectory) < 3:
        return 0

    # 取开始、中间、结束三个点
    start = trajectory[0]
    mid = trajectory[len(trajectory) // 2]
    end = trajectory[-1]

    # 计算方向变化
    vec1 = mid - start
    vec2 = end - mid

    if np.linalg.norm(vec1) == 0 or np.linalg.norm(vec2) == 0:
        return 0

    # 计算夹角（余弦值）
    cos_angle = np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
    angle = np.arccos(np.clip(cos_angle, -1, 1))

    return angle